/*
 * Copyright (c) 1997, 1998, 2003, 2006 Aleksandar Samardzic
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 * 3. The name of the author may not be used to endorse or promote products
 *    derived from this software without specific prior written permission.
 * 
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT,
 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
 * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include "matrix.h"

/* Matrix_SwapRows - swap specified rows of matrix */
void
Matrix_SwapRows(matrix, row0, row1)
     PMatrix         matrix;
     int             row0;
     int             row1;
{
	int             currColumn;
	double          tmp;

	/* swap corresponding row elements */
	for (currColumn = 0; currColumn < 4; currColumn++)
		tmp =
		    matrix[4 * row0 + currColumn],
		    matrix[4 * row0 + currColumn] =
		    matrix[4 * row1 + currColumn],
		    matrix[4 * row1 + currColumn] = tmp;
}

/* Matrix_Invert - Gauss inverse matrix calculation */
Bool
Matrix_Invert(matrix, matrixInv)
     PMatrix         matrix;
     PMatrix         matrixInv;
{
	PMatrix         matrixCopy;
	int             row,
	                col;
	int             currRow,
	                currColumn;
	double          leadingElement,
	                mul;
	Bool            matrixInvExist = TRUE;

	/* initialize matrices */
	for (currRow = 0; currRow < 4; currRow++)
		for (currColumn = 0; currColumn < 4; currColumn++) {
			matrixCopy[4 * currRow + currColumn] =
			    matrix[4 * currRow + currColumn];
			matrixInv[4 * currRow + currColumn] =
			    (currRow == currColumn) ? 1.f : 0.f;
		}

	for (col = 0; col < 4; col++) {
		row = col;

		/* determine leading row */
		if (!matrixCopy[4 * row + col]) {
			do
				row++;
			while (row < 4 && !matrixCopy[4 * row + col]);
			if (row == 4) {	/* all 0s in a row */
				matrixInvExist = FALSE;
				break;
			} else
				Matrix_SwapRows(matrixCopy, col, row),
				    Matrix_SwapRows(matrixInv, col, row);
		}

		/* calculate multiplier */
		leadingElement = matrixCopy[4 * row + col];
		mul = 1 / leadingElement;

		/* multiply row elements in both matrices */
		for (currColumn = 0; currColumn < 4; currColumn++)
			matrixCopy[4 * row + currColumn] *=
			    mul, matrixInv[4 * row + currColumn] *= mul;

		/* perform addition over each col */
		for (currRow = 0; currRow < 4; currRow++)
			if (currRow != row) {
				mul = -matrixCopy[4 * currRow + col];
				for (currColumn = 0; currColumn < 4;
				     currColumn++)
					matrixCopy[4 * currRow +
						   currColumn] +=
					    matrixCopy[4 * row +
						       currColumn] * mul,
					    matrixInv[4 * currRow +
						      currColumn] +=
					    matrixInv[4 * row +
						      currColumn] * mul;
			}
	}

	return matrixInvExist;
}
